﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace RobotsSample
{
    /// <summary>
    /// 操作臂调用示例
    /// </summary>
    public class Sample_ManipulatorRobot : RobotSampleAbstract
    {
        public override void Run()
        {
            base.Run();
            /*
           *######################################################################
           *####################### 机器人模块ID配置  ###########################
           *######################################################################
           */

            //输入操作臂ID次序
            Console.WriteLine("Please enter the manipulator ids in sequence from down to top （I T T I T G）");
            ModulesIds.Clear();

            for (var i = 0; i < 6; i++)
            {
                Console.WriteLine($"Please enter the {i + 1} module's Id");
                int mId = 0;
                var mIdStr = Console.ReadLine();
                int.TryParse(mIdStr, out mId);
                ModulesIds.Add(mId);
            }

            Console.WriteLine("Ids  completed");


            /*
	         *######################################################################
	         *####################### 机器人操作控制开始  ###########################
	         *######################################################################
	         */

            //将关节模式转换成位置模式
            for (var i = 0; i < ModulesIds.Count; i++)
            {
                MiniBotControlLib.SetMotorMode((byte)ModulesIds[i], 0);
            }

            while (true)
            {
                //操作命令
                var cmd = 1;
                Console.WriteLine($"Commands：{Environment.NewLine}1.Set Modules Angles{Environment.NewLine}2.Set Space Coordinates{Environment.NewLine}3.Get Modules Agnles");
                var cmdStr = Console.ReadLine();
                int.TryParse(cmdStr, out cmd);

                switch (cmd)
                {
                    case 1:
                        {

                            var joints = new float[6];
                            for (var i = 0; i < 6; i++)
                            {
                                Console.WriteLine($"Please enter the {(i + 1) } module's angle");
                                float angle;
                                var angleStr = Console.ReadLine();
                                float.TryParse(angleStr, out angle);

                                joints[i] = angle;
                                SetModulesPosition((byte)ModulesIds[i], angle);
                            }

                            Console.WriteLine("Now The Space Coordinates is:");
                            var sc = CalJointAngleToTerminalCartesian(joints);

                            Console.WriteLine($"X：{sc[0]}");
                            Console.WriteLine($"Y：{sc[1]}");
                            Console.WriteLine($"Z：{sc[2]}");
                            Console.WriteLine($"Roll：{sc[3]}");
                            Console.WriteLine($"Pitch：{sc[4]}");
                            Console.WriteLine($"Yaw：{sc[5]}");

                        }
                        break;
                    case 2:
                        {
                            var ps = new float[6];

                            Console.WriteLine("Please enter the Space Coordinates:");
                            Console.WriteLine($"Enter X:");
                            var inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine($"Enter Y:");
                            inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine($"Enter Z:");
                            inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine($"Enter Roll:");
                            inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine($"Enter Pitch:");
                            inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine($"Enter Yaw:");
                            inputStr = Console.ReadLine();
                            float.TryParse(inputStr, out ps[0]);

                            Console.WriteLine("Now The I T T I T Angles is:");
                            var joints = CalTerminalCartesianToJointAngle(ps);

                            //5自由度操作臂
                            for (var i = 0; i < 5; i++)
                            {
                                var angle = joints[i];
                                SetModulesPosition((byte)ModulesIds[i], angle);
                            }

                            Console.WriteLine($" {joints[0]} { joints[1] } { joints[2] } { joints[3] } { joints[4] }");
                        }
                        break;
                    case 3:
                        {
                            //获取关节模块ID
                            Console.WriteLine("Getting modules's angle ");
                            MiniBotControlLib.SetRecvTimeOut(800);
                            for (var i = 0; i < 6; i++)
                            {
                                var angle = GetPosition((byte)ModulesIds[i]);
                                Console.WriteLine($"module{ModulesIds[i]}：{angle}");
                            }
                        }
                        break;
                    default:
                        break;
                }

                Console.WriteLine("Enter any key to continue ....");

            }

        }


        ///<summary>
        ///计算关节角度To末端笛卡尔空间
        ///参数： j[5] 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围 I：正负150角度,T：正负90，G：-90到0  （0度的关节模块0位）
        ///输出：笛卡尔空间 X Y Z Roll Pitch Yaw 六个元素的数组
        /// </summary>
        /// <param name="j"></param>
        /// <returns></returns>
        private float[] CalJointAngleToTerminalCartesian(float[] j)
        {
            if (j.Length < 5)
            {
                return new float[] { 0, 0, 0, 0, 0 };
            }
            var intPtr = FloatArrayToIntptr(j);
            var rs = MiniBotAlgorithmLib.CalJointAngleToTerminalCartesian(intPtr);
            var pos = IntptrToFloatArray(rs, 6);
            return pos;
        }

        /// <summary>
        ///计算末端笛卡尔空间To关节角度
        ///参数： pose[6] X Y Z Roll Pitch Yaw 六个元素的数组
        ///输出：关节弧度角 float* 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围 I：正负150角度,T：正负90，G：-90到0  （0度的关节模块0位）
        /// </summary>
        /// <param name="p"></param>
        /// <returns></returns>
        private float[] CalTerminalCartesianToJointAngle(float[] p)
        {
            if (p.Length < 6)
            {
                return new float[] { 150, 150, 150, 150, 150 };
            }

            var p2 = new float[p.Length];
            for (var i = 0; i < p.Length; i++)
            {
                p2[i] = p[i];
            }


            var intPtrP2 = FloatArrayToIntptr(p2);
            var j2 = MiniBotAlgorithmLib.CalTerminalCartesianToJointAngle(intPtrP2);
            var pos = IntptrToFloatArray(j2, 5);
            return pos;
        }



    }
}
